"""
舵机控制器模块

该模块提供了统一的舵机控制接口，支持PWM和串口两种控制方式。
"""

import time
from typing import Dict, Any, Optional, List
from abc import ABC, abstractmethod
import serial
import pigpio
from robot_core.config.servo_config import ServoConfig, servo_config
from .servos import PWMServo, ServoInterface
from .protocols.serial_protocol import SerialProtocol


class ServoControllerInterface(ABC):
    """舵机控制接口基类"""

    @abstractmethod
    def set_position(self, servo_id: int, position: int, speed: int = 1000) -> bool:
        """设置舵机位置"""
        pass

    @abstractmethod
    def get_position(self, servo_id: int) -> Optional[int]:
        """获取舵机位置"""
        pass

    @abstractmethod
    def emergency_stop(self) -> None:
        """紧急停止"""
        pass

    @abstractmethod
    def close(self) -> None:
        """关闭资源"""
        pass

    @abstractmethod
    def set_servo_parameters(self, servo_id: int, parameters: Dict[str, Any]) -> bool:
        """设置舵机参数

        Args:
            servo_id: 舵机ID
            parameters: 参数配置
                - min_angle: 最小角度
                - max_angle: 最大角度
                - offset: 角度偏移
                - speed_limit: 速度限制

        Returns:
            bool: 是否设置成功
        """
        pass

    @abstractmethod
    def get_servo_parameters(self, servo_id: int) -> Optional[Dict[str, Any]]:
        """获取舵机参数

        Args:
            servo_id: 舵机ID

        Returns:
            Optional[Dict[str, Any]]: 舵机参数,如果获取失败返回None
        """
        pass

    @abstractmethod
    def get_all_servo_positions(self) -> Dict[int, int]:
        """获取所有舵机的位置

        Returns:
            Dict[int, int]: 舵机ID到位置的映射
        """
        pass


class PWMServoController(ServoControllerInterface):
    """PWM舵机控制器"""

    def __init__(self, config: Dict[str, Any]):
        self.config = config
        self.pi = pigpio.pi()
        self.servos: Dict[int, ServoInterface] = {}
        self._init_servos()

    def _init_servos(self) -> None:
        """初始化所有PWM舵机"""
        servo_config = self.config.get("servo", {})
        for servo_id, pin in servo_config.get("pins", {}).items():
            self.servos[servo_id] = PWMServo(
                self.pi,
                pin,
                freq=self.config.get("freq", 50),
                min_width=self.config.get("min_width", 500),
                max_width=self.config.get("max_width", 2500),
                deviation=self.config.get("deviation", 0),
                control_speed=self.config.get("control_speed", True),
            )

    def set_position(self, servo_id: int, position: int, speed: int = 1000) -> bool:
        """设置PWM舵机位置"""
        if servo_id in self.servos:
            self.servos[servo_id].set_position(position, speed)
            return True
        return False

    def get_position(self, servo_id: int) -> Optional[int]:
        """获取PWM舵机位置"""
        if servo_id in self.servos:
            return self.servos[servo_id].get_position()
        return None

    def emergency_stop(self) -> None:
        """紧急停止所有PWM舵机"""
        for servo in self.servos.values():
            servo.set_position(0, 0)

    def close(self) -> None:
        """关闭PWM舵机资源"""
        for servo in self.servos.values():
            servo.set_position(0, 0)
        self.pi.stop()

    def set_servo_parameters(self, servo_id: int, parameters: Dict[str, Any]) -> bool:
        """设置PWM舵机参数"""
        if servo_id in self.servos:
            servo = self.servos[servo_id]
            if "min_angle" in parameters:
                servo.min_angle = parameters["min_angle"]
            if "max_angle" in parameters:
                servo.max_angle = parameters["max_angle"]
            if "offset" in parameters:
                servo.set_deviation(parameters["offset"])
            if "speed_limit" in parameters:
                servo.speed_limit = parameters["speed_limit"]
            return True
        return False

    def get_servo_parameters(self, servo_id: int) -> Optional[Dict[str, Any]]:
        """获取PWM舵机参数"""
        if servo_id in self.servos:
            servo = self.servos[servo_id]
            return {
                "min_angle": servo.min_angle,
                "max_angle": servo.max_angle,
                "offset": servo.deviation,
                "speed_limit": servo.speed_limit,
            }
        return None

    def get_all_servo_positions(self) -> Dict[int, int]:
        """获取所有PWM舵机的位置"""
        return {
            servo_id: servo.get_position() for servo_id, servo in self.servos.items()
        }


class SerialServoController(ServoControllerInterface):
    """串口舵机控制器"""

    def __init__(self, port: str = "/dev/ttyAMA0", baudrate: int = 115200):
        """
        初始化舵机控制器

        Args:
            port: 串口设备名
            baudrate: 波特率
        """
        print(f"初始化舵机控制器: {port}, 波特率: {baudrate}")
        self.protocol = SerialProtocol(port, baudrate)
        self.config = ServoConfig(self.protocol)
        self.servo_count = self.config.get_serial_config().get("servo_count", 18)
        self.servo_range = self.config.get_serial_config().get(
            "servo_range", {"min": 0, "max": 1000}
        )
        self.positions = {i: 500 for i in range(1, self.servo_count + 1)}
        self.servo_config = ServoConfig()
        self._init_serial()

    def _init_serial(self) -> None:
        """初始化串口"""
        try:
            self.serial = serial.Serial(
                port=self.protocol.port, baudrate=self.protocol.baudrate, timeout=1
            )
        except Exception as e:
            print(f"串口初始化失败: {e}")
            self.serial = None

    def set_position(self, servo_id: int, position: int, speed: int = 1000) -> bool:
        """设置串口舵机位置"""
        if not 1 <= servo_id <= self.servo_count:
            return False

        try:
            position = max(
                self.servo_range["min"], min(position, self.servo_range["max"])
            )
            cmd = f"#{servo_id}P{position}S{speed}\r\n"
            self.serial.write(cmd.encode())
            self.positions[servo_id] = position
            return True
        except Exception as e:
            print(f"发送舵机命令失败: {e}")
            return False

    def get_position(self, servo_id: int) -> Optional[int]:
        """获取串口舵机位置"""
        return self.positions.get(servo_id)

    def emergency_stop(self) -> None:
        """紧急停止所有串口舵机"""
        if self.serial and self.serial.is_open:
            try:
                self.serial.write(b"#0P0S0\r\n")
            except Exception as e:
                print(f"紧急停止失败: {e}")

    def close(self) -> None:
        """关闭串口舵机资源"""
        if self.serial and self.serial.is_open:
            self.serial.close()
        self.servo_config.close()

    def set_servo_parameters(self, servo_id: int, parameters: Dict[str, Any]) -> bool:
        """设置串口舵机参数"""
        if not 1 <= servo_id <= self.servo_count:
            return False

        try:
            if "min_angle" in parameters:
                self.servo_range["min"] = parameters["min_angle"]
            if "max_angle" in parameters:
                self.servo_range["max"] = parameters["max_angle"]
            if "offset" in parameters:
                cmd = f"#{servo_id}O{parameters['offset']}\r\n"
                self.serial.write(cmd.encode())
            if "speed_limit" in parameters:
                cmd = f"#{servo_id}S{parameters['speed_limit']}\r\n"
                self.serial.write(cmd.encode())
            return True
        except Exception as e:
            print(f"设置舵机参数失败: {e}")
            return False

    def get_servo_parameters(self, servo_id: int) -> Optional[Dict[str, Any]]:
        """获取串口舵机参数"""
        if not 1 <= servo_id <= self.servo_count:
            return None

        try:
            cmd = f"#{servo_id}?\r\n"
            self.serial.write(cmd.encode())
            response = self.serial.readline().decode().strip()
            # 解析响应获取参数
            # TODO: 根据实际串口协议实现参数解析
            return {
                "min_angle": self.servo_range["min"],
                "max_angle": self.servo_range["max"],
                "offset": 0,  # 需要从响应中解析
                "speed_limit": 1000,  # 需要从响应中解析
            }
        except Exception as e:
            print(f"获取舵机参数失败: {e}")
            return None

    def get_all_servo_positions(self) -> Dict[int, int]:
        """获取所有串口舵机的位置"""
        return self.positions.copy()


class ServoController:
    """统一的舵机控制器"""

    def __init__(self, port: str = "/dev/ttyS0", baudrate: int = 115200):
        """
        初始化舵机控制器

        Args:
            port: 串口设备名
            baudrate: 波特率
        """
        print(f"初始化舵机控制器: {port}, 波特率: {baudrate}")
        self.config = ServoConfig()
        self.config.init_serial_protocol(port, baudrate)
        self.control_type = self.config.get_control_type()  # 默认使用串口控制

        # 根据控制类型创建对应的控制器
        if self.control_type == "pwm":
            self.controller = PWMServoController(self.config)
        else:
            self.controller = SerialServoController(port, baudrate)

    def set_position(self, servo_id: int, position: int, speed: int = 1000) -> bool:
        """设置舵机位置"""
        return self.controller.set_position(servo_id, position, speed)

    def get_position(self, servo_id: int) -> Optional[int]:
        """获取舵机位置"""
        return self.controller.get_position(servo_id)

    def emergency_stop(self) -> None:
        """紧急停止所有舵机"""
        self.controller.emergency_stop()

    def close(self) -> None:
        """关闭舵机控制器"""
        self.controller.close()

    def set_servo_parameters(self, servo_id: int, parameters: Dict[str, Any]) -> bool:
        """设置舵机参数"""
        return self.controller.set_servo_parameters(servo_id, parameters)

    def get_servo_parameters(self, servo_id: int) -> Optional[Dict[str, Any]]:
        """获取舵机参数"""
        return self.controller.get_servo_parameters(servo_id)

    def get_all_servo_positions(self) -> Dict[int, int]:
        """获取所有舵机的位置"""
        return self.controller.get_all_servo_positions()


# 创建全局舵机控制器实例
servo_controller = ServoController()  # 使用默认参数初始化
